from pathlib import Path

from xensesdk.xenseInterface.XenseSensor import Sensor
from ezrobot.sensor.base import BaseSensor
# from line_profiler import LineProfiler

class TactileSensor(BaseSensor):
    def __init__(self, serial_num: str, return_data_types: list = list(['src', 'force'])):
        self.index = serial_num
        self.sensor = Sensor.create(serial_num, check_serial=False, config_path=Path(__file__).parents[2]/f"configs/{serial_num}")
        self.return_data_types = return_data_types
    # @profile
    def recv_data(self, **kwargs):
        tactile_dict = {}
        types = []
        for require_type in self.return_data_types:
            if require_type == "force":
                types.append(Sensor.OutputType.Force)
            elif require_type == "src":
                types.append(Sensor.OutputType.Rectify)    
            elif require_type == "depth":
                types.append(Sensor.OutputType.Depth)
            else:
                raise ValueError(f"{require_type} is not a unsupported type")
        data = self.sensor.selectSensorInfo(*types)
        for i, require_type in enumerate(self.return_data_types):
            tactile_dict[require_type] = data[i]
        return tactile_dict


if __name__ == "__main__":
    ser_num = 'OG000053'
    tactile_capture = TactileSensor(ser_num)
    for _ in range(100):
        d = tactile_capture.recv_data()
